1. Field of the Invention
The present invention relates in general to inertial navigation systems (INS), and more particularly to a kinematics equations integrator device and method for obtaining the attitude of a vehicle from the closed-form integration of kinematics equations utilizing a four-dimensional operator such that the magnitude of said four-dimensional operator is always equal to unity.
2. Background Art
Inertial navigation systems (INS) are widely used in several flight vehicle applications such as aircraft, missiles, spacecraft and satellites. These self-contained navigation systems determine the attitude or position of the flight vehicle relative to some reference coordinate frame. The INS works by integrating the angular velocity or rates of acceleration, measured by onboard sensors, to obtain position with respect to a body-centered coordinate frame.
In order to determine the attitude of vehicle relative to Earth-centered coordinates, the body axes are transformed into a Earth-axes coordinate frame by using the Euler angles. The Euler angles are angles through which one coordinate frame must be rotated to bring its axes to coincidence with another coordinate frame. Euler angles describe the body axes coordinates, namely longitudinal (roll), lateral (pitch) and normal (yaw) axes, with respect to Earth axes coordinates, or the local tangent plane of the Earth and true north, namely north, east and down axes. FIG. 1 illustrates the body axes and the Earth axes coordinate systems.
The direction cosine matrix that allows the transformation from Earth axes to body axes is the product of three successive rotations about the pitch, roll and yaw axes. The transformation is given by the equation: EQU V.sub.body =M[.theta., .phi., .psi.]V.sub.Earth (1)
where M(.theta.,.phi.,.psi.) is the direction cosine matrix, ##EQU1## Here, .theta. is the pitch angle, .phi. is the roll or bank angle, and .psi. is the yaw or heading angle.
The onboard sensors used to measure the angular velocity about the pitch, roll and yaw axes are usually accelerometers and gyroscopes. For most INS applications, these sensors are mounted on the vehicle in one of two ways: the platform INS or the strap-down INS. The platform INS maintains the sensors in the same attitude relative to the surroundings. This is achieved by placing the sensors in a gimbal housings. Depending on the application several gimbal housings may be required.
The strap-down INS fixes the sensors on each of the three body axes and does not require gimbal housings. As a result, the strap-down INS has lower weight, lower power consumption and higher accuracy than the platform INS. Consequently, in most modern applications the strap-down INS is preferred. Because, however, of the need to calculate coordinate transformations, the strap-down INS places maximum burden on the computational system. Thus, high-speed processors, significant amounts of computer memory and complex software generally are required for the strap-down INS.
One recurring problem of both types of INS is that they are neutrally stable systems and any bias or systematic errors in the angular velocity measurements remain and are not damped out. This causes the INS to drift, meaning that the error increases with time. One solution is to use the platform INS and stabilize the sensors in gimbal housings. The problem is this arrangement is subject to a physical locking up of the gimbal when the pitch angle of the vehicle is vertical, or 90 degrees. This "gimbal lock," as it is known in the art, is particularly problematic for missiles, spacecraft and other flight vehicles that often assume a pitch angle of 90 degrees for extended periods of time. Thus, platform INS is rarely used in modem applications.
Strap-down INS is not subject to a physical "gimbal lock" like the platform INS. However, there are other problems with the strap-down INS. First, because the kinematics equations have a division by zero when the pitch angle is 90 degrees, the strap-down INS is subject to a mathematical "gimbal lock." For example, the measured angular velocity vector (p, q, r) in body axes, where p is the roll rate, q is the pitch rate, and r is the yaw rate, is expressed in Earth axes by the following kinematics equations: ##EQU2## In equation 3, the 1/cos .theta. term has a singularity when the pitch angle .theta. passes through .+-..pi./2, in other words when the pitch angle is vertical. This singularity makes equation 3 difficult to integrate to obtain the attitude of the vehicle.
Second, in addition to the singularity problem, there is computational difficulty in trying to obtain the Euler angles from the integration of equation 3. In general, no closed-form solution for equation 3 exist and it must be numerically integrated. However, equation 3 is highly non-linear, and the sine and cosine terms must be evaluated as Taylor series expansions. For example, the numerical integration of equation 3 using a fourth-order numerical integration scheme requires that each sine and cosine term be evaluated four different times at each time step. These computations require a great deal of computational expense and time.
The prior art avoids these foregoing problems in integrating equation 3 by formulating the kinematics equations in terms of quaternion parameters. In general, a quaternion is a four-element vector with one real and three imaginary components. The quaternion provides a four-parameter operation of coordinate transformation that is a more efficient representation for rotation than the three-dimensional orthonormal matrix with nine parameters and six orthonormal constraints. The quaternion formulation transforms three quantities into four quantities with only one constraint. This four-space vector and constraint can be written as: ##EQU3## Where, u.sub.x, u.sub.y, u.sub.z, are the components of the unit vectors pointing along the body axis of the vehicle, and the one constraint is that the norm of the quaternion be equal to unity.
The quaternion formulation transforms the three-dimensional attitude of pitch, roll and yaw (.theta., .phi., .psi.) into a set of quaternion parameters (a,b,c,d) with the single constraint. Instead of the usual three rotations, using quaternions accomplishes the coordinate transformation from Earth axes to body axes in a single rotation.
The quaternions are defined in terms of the direction cosine matrix of equation 2 as: ##EQU4## and the direction cosine matrix, equation 2, reduces to: ##EQU5##
In geometric terms, the quaternion formulation maps the kinematic equations of three-dimensional space onto the surface of a unit hypersphere in four-dimensional space, with the constraint that the radius of the hypersphere is equal to unity. The result is that non-linear sine and cosine terms as well as the singularity of equation 3 disappear. The3-D kinematics equations of equation 3 become in terms of a 4-D quaternion formulation: ##EQU6## Equation 8 can be written in vector form as: EQU .lambda.(t)=.omega.(p,q,r) .lambda.(t) (9)
Equation 9 looks like a linear, time-varying system common in dynamics. However, integration of equation 9 is actually quite difficult because of the unique properties of quaternions. Quaternion parameters are on the surface of a unit hypersphere in four-dimensional space. All operations, therefore, must be rotational and must take place on the surface of the unit hypersphere. This non-Euclidean quaternions space doe., not follow the rules of vector algebra. Essentially, normal linear Euclidean operations of addition and subtraction do not exist in quaternion space.
Traditional approaches of the prior art in solving the quaternion kinematics equations of equation 9 have been to use numerical integration schemes such as Runge-Kutta and Adams-Bashforth. These methods, however, violate the mathematics of the quaternion space. Nevertheless, the prior art continues to use these and a number of other numerical integration schemes to integrate the quaternion kinematics equations.
All of these numerical integration schemes require the approximation of integrals by summation. But because addition does not exist in the quaternion space, the single constraint that a+b+c+d=1 (i.e., normalization of the quaternion) is not guaranteed. Geometrically, this occurs because addition is not a rotational operation, and the summation performed by the integration scheme does not take place on the surface of tie unit hypersphere. Therefore, the integration scheme must renormalize the quaternions after each time step to ensure the quaternion norm is equal to unity.
The problem with renormalization after each time step is that it introduces error into the integration in the form of analytical drift. Left unchecked, this drift accumulates over time and eventually leads to divergence of the integration and instability. The prior art integration schemes manage this drift through a variety of ad hoc methods. Usually these ad hoc methods involve trading error between the axes by adding or subtracting correction terms at each time step in order to artificially preserve quaternion normality.
The primary disadvantage of the prior art integration schemes is that none actually preserve quaternion normality. In fact, the best that these method can do is to correct the drift in quaternion normalization after it has occurred. This renormalization procedure, however, is an artificial operation that violates the mathematics of the quaternion space, and therefore always introduces additional errors into the integration. Furthermore, the need to renormalize the quaternions after every time step greatly reduces the integration speed.
Another disadvantage to existing numerical integration schemes is that most of them are proprietary. Each method is designed for a specific computational system based on the noise of the integration, noise of the system and how many bits contained in the processor. This means an integration scheme designed for a specific computational system may not work on another computational system, thereby reducing the portability of the integration scheme.
Still another disadvantage to prior art integration schemes is that powerful and expensive computational systems are needed to implement them. This is because the error from the normalization drift must be corrected by performing several operations on various terms in the equation after each time step. This need for additional computational capability can add a great deal of weight to the INS and to the vehicle.
Another disadvantage of the prior art schemes is that the lines of source code and the complexity of the software required to implement these integration methods are generally quite large. The memory, therefore, required to store this software is substantial. In addition, the cost of code maintainability is high because of the length and complexity of the source code. Moreover, if transportation of the code between computational systems requires the code to be in a different language the cost of rewriting the code in another language can be high.
Therefore, what is needed is a kinematics equations integrator device and method that preserves the quaternion normalization. This closed form integrator device and method would obey the mathematical properties of the quaternion space and therefore would not require correction terms or renormalization ever. Moreover, because the need for renormalization and correction terms would not exist, this integrator device and method would greatly increase the integration speed, decrease the amount and complexity of the software required, and require only basic computational systems on which to operate. Further, the integrator device and method would be portable between-various computational systems.
Whatever the merits of the above mentioned systems and methods, they do not achieve the benefits of the present invention.